/* 
* @Author: obsidian
* @Date:   2015-12-15 19:59:08
* @Last Modified by:   obsidian
* @Last Modified time: 2016-04-02 09:04:52
*/

#include "steer.h"
#include "driver/sd/file.h"
#include "steer_driver.h"
#include "control/car.h"
#include "../def.h"


static void steer_init(void);
static void steer_save(void);
static void steer_set_angle(float angle);
static void steer_read_sd(void);

struct Steer steer =
{
        .CCRx                =   (unsigned short *)&TIM4->CCR4,         //CCRx  short
        .status              =   0,                                    //status  s char
        .midvalue         =   1170,                                   //midvalue short
        .limit                  =   {1600, 1400},                          //limit[2]
        .ratio                 =   -3.6,                                    //ratio
        .angle                =   0.0,
        .init                    =   steer_init,
        .read_from_sd  =   steer_read_sd,                      //
        .set_angle         =   steer_set_angle,                       //
        .save                 =   steer_save,
};


void steer_set_angle(float angle)
{
        if(steer.status == OFF) {return;}
        steer.angle = angle;
        unsigned short pwm = 0;

        pwm  = steer.midvalue + (signed short)(steer.ratio * angle);
        if(pwm > steer.limit[0]) 
        {
            pwm = steer.limit[0];
        }
        else if(pwm < steer.limit[1]) 
        {
            pwm = steer.limit[1];
        }
        *steer.CCRx = pwm;
}

struct Steer dup_steer;

void steer_init()
{
//    TIM4_Mode_Config();
}

void steer_read_sd()
{
    unsigned int k = 0;

    car.strjoint("steer");
    f_open(&fil,car.path,FA_READ);
    if(fil.fs != 0)
    {
        f_read(&fil, (char *)&dup_steer, sizeof(struct Steer), &k);
        
        steer.midvalue = dup_steer.midvalue;
        steer.limit[0] = dup_steer.limit[0];
        steer.limit[1] = dup_steer.limit[1];
        steer.ratio = dup_steer.ratio;
        file_close();
    }    
}

void steer_save()
{
    unsigned int k = 0;
    car.strjoint("steer");
      file_open(car.path);
    if(fil.fs == 0){return;}
    f_write(&fil, (char *)&steer, sizeof(struct Steer), &k);
    file_close();
}
